//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#include "HectorMappingRos.h"

#include <boost/thread/locks.hpp>

#include <pcl/ros/conversions.h>
#include <pcl_ros/transforms.h>

#include "google_mapping/Map.h"

HectorMappingRos::HectorMappingRos()
    : pose_(tf::Pose::getIdentity()),
      first_point_cloud_inserted_(false),
      motion_model_(5., 4.8) {
  ros::NodeHandle nh("~");
  nh.param("base_frame", p_base_frame_, std::string("base_link"));
  nh.param("map_frame", p_map_frame_, std::string("map"));
  nh.param("map_pub_period", p_map_pub_period_, 2.0);
  nh.param("map_resolution", p_map_resolution_, 0.025);
  nh.param("map_hit_probability", p_map_hit_probability_, 0.7);
  nh.param("map_miss_probability", p_map_miss_probability_, 0.4);
  nh.param("map_minimum_occupancy", p_map_minimum_occupancy_, 0.12);
  nh.param("map_maximum_occupancy", p_map_maximum_occupancy_, 0.97);
  nh.param("laser_z_max_value", p_laser_z_max_value_, 1.);
  nh.param("laser_z_min_value", p_laser_z_min_value_, -1.);

  ROS_INFO("google_mapping: base_frame: %s", p_base_frame_.c_str());
  ROS_INFO("google_mapping: map_frame: %s", p_map_frame_.c_str());
  ROS_INFO("google_mapping: map_pub_period: %f", p_map_pub_period_);
  ROS_INFO("google_mapping: map_resolution: %f", p_map_resolution_);
  ROS_INFO("google_mapping: map_hit_probability: %f", p_map_hit_probability_);
  ROS_INFO("google_mapping: map_miss_probability: %f", p_map_miss_probability_);
  ROS_INFO("google_mapping: map_minimum_occupancy: %f", p_map_minimum_occupancy_);
  ROS_INFO("google_mapping: map_maximum_occupancy: %f", p_map_maximum_occupancy_);
  ROS_INFO("google_mapping: laser_z_max_value: %f", p_laser_z_max_value_);
  ROS_INFO("google_mapping: laser_z_min_value: %f", p_laser_z_min_value_);

  map_.reset(new google_mapping::Map(p_map_resolution_));
  map_->setHitProbability(p_map_hit_probability_);
  map_->setMissProbability(p_map_miss_probability_);
  map_->setMinimumOccupancy(p_map_minimum_occupancy_);
  map_->setMaximumOccupancy(p_map_maximum_occupancy_);

  map_vis_.reset(new google_mapping::MapVisualization(map_.get()));

  scan_subscriber_ = node_.subscribe(
      "scan", 1, &HectorMappingRos::scanCallback, this);
  pointcloud_subscriber_ = node_.subscribe(
      "cloud", 1, &HectorMappingRos::cloudCallback, this);

  syscommand_subscriber_ = node_.subscribe(
      "syscommand", 1, &HectorMappingRos::syscommandCallback, this);

  map_publisher_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  map_publisher_thread_.reset(
      new boost::thread(
          boost::bind(
              &HectorMappingRos::publishMapLoop, this, p_map_pub_period_)));
}

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan) {
  if (!tf_listener_.waitForTransform(
      p_base_frame_,
      scan.header.frame_id,
      scan.header.stamp,
      ros::Duration(3.))) {
    ROS_INFO("TF not ready.");
    return;
  }
  if (!tf_listener_.waitForTransform(
      p_base_frame_,
      scan.header.frame_id,
      scan.header.stamp + ros::Duration(scan.scan_time),
      ros::Duration(3.))) {
    ROS_INFO("TF not ready.");
    return;
  }
  sensor_msgs::PointCloud2 point_cloud_message;
  laser_projection_.transformLaserScanToPointCloud(
      p_base_frame_, scan, point_cloud_message, tf_listener_);
  pcl::PointCloud<pcl::PointXYZ> point_cloud;
  pcl::fromROSMsg(point_cloud_message, point_cloud);
  // Currently we are building a 2D map and using a 2D scan matcher. So, we
  // throw out large Z values and flatten the rest.
  pcl::PointCloud<pcl::PointXYZ>::iterator it = point_cloud.begin();
  while (it != point_cloud.end()) {
    if (it->z > 1 || it->z < -1) {
      it = point_cloud.erase(it);
    } else {
      it->z = 0;
      ++it;
    }
  }
  processPointcloud(point_cloud, scan.header.stamp);
}

void HectorMappingRos::cloudCallback(const sensor_msgs::PointCloud2& cloud)
{
  //sensor_msgs::PointCloud2 cloud_base_frame;
  //pcl_ros::transformPointCloud	(p_base_frame_, cloud, cloud_base_frame, tf_listener_);
  pcl::PointCloud<pcl::PointXYZ> cloud_pcl;
  pcl::fromROSMsg(cloud, cloud_pcl);

  pcl::PointCloud<pcl::PointXYZ> cloud_pcl_base_frame;
  pcl_ros::transformPointCloud	(p_base_frame_, cloud_pcl, cloud_pcl_base_frame, tf_listener_);

  processPointcloud(cloud_pcl_base_frame, cloud.header.stamp);
}

void HectorMappingRos::processPointcloud(pcl::PointCloud<pcl::PointXYZ>& point_cloud, const ros::Time& acquisition_stamp)
{
  {
    boost::lock_guard<boost::mutex> lock_guard(mutex_);
    if (!first_point_cloud_inserted_) {
      map_->insertPointCloud(point_cloud, pose_);
      first_point_cloud_inserted_ = true;
      last_inserted_scan_time_ = acquisition_stamp;
      return;
    }

    if (!scan_matcher_.match(*map_, point_cloud, motion_model_, acquisition_stamp, &pose_)) {
      ROS_WARN("Pose estimation failed.");
      return;
    }
    //ros::shutdown();

    ros::Duration time_delta = acquisition_stamp                       - last_inserted_scan_time_;
    if (time_delta > ros::Duration(0.5) || poseDifferenceLargerThan(
          last_inserted_point_cloud_pose_, pose_, 0.4, 0.9)) {
      map_->insertPointCloud(point_cloud, pose_);
      last_inserted_point_cloud_pose_ = pose_;
      last_inserted_scan_time_ = acquisition_stamp;
    }
  }
  motion_model_.addPoseObservation(acquisition_stamp, pose_);
  tf_broadcaster_.sendTransform(
      tf::StampedTransform(
          pose_, acquisition_stamp, p_map_frame_, p_base_frame_));

}

bool HectorMappingRos::poseDifferenceLargerThan(
  const tf::Pose& pose1,
  const tf::Pose& pose2,
  double distance_threshold,
  double angle_threshold) {
  double distance = pose1.getOrigin().distance(
      pose2.getOrigin());
  double angle = abs(pose1.getRotation().angle(pose2.getRotation()));
  return distance > distance_threshold || angle > angle_threshold;
}

void HectorMappingRos::publishMap(const ros::Time& time) {
  boost::lock_guard<boost::mutex> lock_guard(mutex_);
  nav_msgs::OccupancyGrid message;
  map_->getOccupancyGrid(&message);
  message.header.stamp = time;
  message.header.frame_id = p_map_frame_;
  map_publisher_.publish(message);
  map_vis_->publishVisualization();
}

void HectorMappingRos::publishMapLoop(double map_pub_period) {
  ros::Rate rate(1.f / map_pub_period);
  while(ros::ok()) {
    ros::Time mapTime (ros::Time::now());
    publishMap(mapTime);
    rate.sleep();
  }
}

void HectorMappingRos::syscommandCallback(const std_msgs::String& string) {
  ROS_INFO("google_mapping: received syscommand %s", string.data.c_str());
  if (string.data == "reset") {
    ROS_INFO("google_mapping: reset");
    pose_ = tf::Pose::getIdentity();
    map_->reset();
  }
}

